
/*****************************************************************************************/
/*  < wifibotPosition.cpp is a program for claculating the position using wheel odometry and
 * orientation from vectornav, 

 
  This program also publishes the position of type nav_msgs/Odometry. It is also intended to be use in 
  robot_pose_ekf thus the topic publishes will be named as /odom

  <For this program it is necessary to use wifibot and vectornav, if you dont have vectornav you can use
  the wifibot.cpp topic odom. If you use this package then run the wifibot_node using launch file as it renames 
  the topic /odom so that robot_pose_ekf uses this odom>
  
   <Copyright (C) <iroboApp - 2013>  
    
    <Authors: Anis Koubaa <akoubaa@coins-lab.org> , Yasir Javed <yasir.javed@coins-lab.org> 

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/
/*****************************************************************************************/

#include "ros/ros.h"
# include "wifibotPosition.h"

using namespace std;

    // define static data members at global namespace scope
    //all static data members must be defined at global scope
    ros::Publisher wifibotPosition::_positionWifibotPublisher;
    ros::Subscriber wifibotPosition::subscriberStatus;
    ros::Subscriber wifibotPosition::subscriberVN100;
   
    // Status Report
    robotControl::WStatus wifibotPosition::wifibotGlobalStatus;
    robotControl::vn100_msg wifibotPosition::vnGlobalStatus;
    nav_msgs::Odometry wifibotPosition::wifibotPose;
    ODOMETRY PreviousOdometry=0.0;

    
/**********************************************************/
// Function degreeToRadian 
// Input: BEARING
//Output : BEARING
/**********************************************************/
BEARING degreeToRadian (BEARING deg ) {
  
  return deg * (PI / 180.0);
}    


/**********************************************************/
// Function statusCallback 
// Input: robotControl::WStatus
//Output : update the wifibotGlobalStatus global variable with data published by (wifibot_node)
/**********************************************************/
void wifibotPosition::statusCallback(robotControl::WStatus msg){
  // Global Status of wifibot is updated
  wifibotGlobalStatus.battery_level  = msg.battery_level;
  wifibotGlobalStatus.current =  msg.current;
  wifibotGlobalStatus.ADC1 = msg.ADC1;
  wifibotGlobalStatus.ADC2 =  msg.ADC2;
  wifibotGlobalStatus.ADC3 = msg.ADC3;
  wifibotGlobalStatus.ADC4 = msg.ADC4;
  wifibotGlobalStatus.speed_front_left =  msg.speed_front_left;
  wifibotGlobalStatus.speed_front_right =  msg.speed_front_right;
  wifibotGlobalStatus.odometry_left =msg.odometry_left;
  wifibotGlobalStatus.odometry_right = msg.odometry_right ;
  wifibotGlobalStatus.version  =  msg.version;
}

/*********************************************************************************/
// Function vnStatusCallback
// Input: robotControl::vn100_msg
//Output : update the vnGlobalStatus global variable with data published by vectornav_node
/********************************************************************************/
void wifibotPosition::vnStatusCallback(robotControl::vn100_msg msg){
  vnGlobalStatus.yaw = msg.yaw;
  vnGlobalStatus.pitch = msg.pitch;
  vnGlobalStatus.roll = msg.roll;
}

/*********************************************************************************/
// Function CalculatePosition
// Input: none
//Output : update and publish the position of the Wifibot using Odometry and Yaw values.
/********************************************************************************/
int wifibotPosition::CalculatePosition(){
	
	DISTANCE travelledDistance;
	COORDINATE dx,dy;	
	BEARING tmpYaw;
	travelledDistance =  (((static_cast<DISTANCE>(wifibotGlobalStatus.odometry_left)+static_cast<DISTANCE>(wifibotGlobalStatus.odometry_right))/2)-static_cast<DISTANCE>(PreviousOdometry));
	tmpYaw = degreeToRadian(-vnGlobalStatus.yaw);
	//Calculate position change for x and y
	dx = (travelledDistance * cos(tmpYaw)); 
	dy = (travelledDistance * sin(tmpYaw));
	
	//Calculate the new position of the robot
	wifibotPose.pose.pose.position.x =(wifibotPose.pose.pose.position.x + dx);
	wifibotPose.pose.pose.position.y= (wifibotPose.pose.pose.position.y + dy);
	wifibotPose.pose.pose.position.z = 0;
	
	//Store the current wheel odometry for distance calculation
	PreviousOdometry = ((wifibotGlobalStatus.odometry_left+wifibotGlobalStatus.odometry_right)/2);
	
	//init position covariance matrix to zero 
	 for (int i=1;i<36;i++) 
	  wifibotPose.pose.covariance[i]=0;
	  
	  //set diagonal values; these values are taken fron Turtlebot code 
	  wifibotPose.pose.covariance[0] = 1e-3;
	  wifibotPose.pose.covariance[7] = 1e-3;
	  wifibotPose.pose.covariance[14] = 1e6;
	  wifibotPose.pose.covariance[21] = 1e6;
	  wifibotPose.pose.covariance[28] = 1e6;
	  wifibotPose.pose.covariance[35] = 1e3; 
	 
	  // adapted from wifibot.cpp in pacakage ROSWIFIBOT	 
	  wifibotPose.twist.twist.linear.x = ((wifibotGlobalStatus.speed_front_left +  wifibotGlobalStatus.speed_front_right) /2);
	  wifibotPose.twist.twist.linear.y = 0.0;
	  wifibotPose.twist.twist.linear.x = ((wifibotGlobalStatus.speed_front_right -  wifibotGlobalStatus.speed_front_left) /0.30); // the 0.30 is the entrax
	  
	  //init twist covariance matrix to zero 
	  for (int i=1;i<36;i++) 
	    wifibotPose.twist.covariance[i]=0;
	  
	  //set diagonal values; these values are taken fron Turtlebot code   
	    wifibotPose.twist.covariance[0] = 1e-3;
	    wifibotPose.twist.covariance[7] = 1e-3;
	    wifibotPose.twist.covariance[14] = 1e6;
	    wifibotPose.twist.covariance[21] = 1e6;
	    wifibotPose.twist.covariance[28] = 1e6;
	    wifibotPose.twist.covariance[35] = 1e3;  
	    
	//publish the updated position
	    _positionWifibotPublisher.publish(wifibotPose);

	return 1;
}

////////////////////////The Main function ///////////////////////////////////
int main(int argc, char **argv)
{
   
   ros::init(argc, argv, "positionPublish");
   ros::NodeHandle n;
   ROS_INFO("Starting wifibotPosition.cpp!");
    wifibotPosition wpos;
 
  //publish the speed
   wifibotPosition::_positionWifibotPublisher= n.advertise<nav_msgs::Odometry>("odom",1);
   //subscribe to vn100 topic published by wifibot.cpp
   wifibotPosition::subscriberVN100 = n.subscribe("vn_ypr_topic", 2, &wifibotPosition::vnStatusCallback, &wpos);
   //subscribe to wifibot status topic published by wifibot.cpp
   wifibotPosition::subscriberStatus = n.subscribe("status", 2, &wifibotPosition::statusCallback, &wpos);
  
  
      ros::AsyncSpinner statusSpinner(3);
      statusSpinner.start();
      ros::Rate threadRate(10);
   
  while (ros::ok()){ // rosok bracket
   
            wpos.CalculatePosition();
 	    threadRate.sleep();

  }
  statusSpinner.stop();
  return 0;
}